
#include "mpu6050.h"
/*-----------原始数据-----------*/
short accelX, accelY, accelZ;            // 三轴加速度
short gyroX, gyroY, gyroZ;               //三轴角速度
/*-----------输出数据-----------*/
short g_fGyro_X,g_fGyro_Y,g_fGyro_Z;     //角速度数据
short g_faccel_X,g_faccel_Y,g_faccel_Z;  //角加速度数据
short angle_X,angle_Y,angle_Z;
/*-------MPU6050初始化函数-------*/
void mpu6050_begin() {
/*----------I2C初始化-----------*/
  Wire.begin();
/*----------解除休眠状态---------*/
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
/*-------陀螺仪采样率，1KHz------*/           
  Wire.beginTransmission(0x68);
  Wire.write(0x19);
  Wire.write(0x07);
  Wire.endTransmission();
/*--------低通滤波器设置---------*/
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x04);
  Wire.endTransmission();
/*------角速度检测范围(±2g)------*/                        
  Wire.beginTransmission(0x68); 
  Wire.write(0x1C);
  Wire.write(0x01);
  Wire.endTransmission();
/*---角速度检测范围(±2000deg/s)--*/           
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x18);
  Wire.endTransmission();
  }
/*----------加速度数据-----------*/
void recordAccelRegisters() {
  Wire.beginTransmission(0x68);          //I2C 将数据写入的地址0x68
  Wire.write(0x3B);                      //I2C向地址写入的数据
  Wire.endTransmission();                //I2C结束写数据
  Wire.requestFrom(0x68,6);              //I2C从0x68地址读取6个数据
  while(Wire.available() < 6);           // 等待6个字节的数据全部存储好 （必须等待所有数据存储到缓冲区后才能读取） 
  accelX = Wire.read()<<8|Wire.read();   //第一个字节和第二个字节是X轴的加速度数据
  accelY = Wire.read()<<8|Wire.read();   //第三个字节和第四个字节是Y轴的加速度数据
  accelZ = Wire.read()<<8|Wire.read();   //第五个字节和第六个字节是Z轴的加速度数据

  g_faccel_X = accelX/16384;             // 量程为±2g时，灵敏度：16384 LSB/g
  g_faccel_Y = accelY/16384;             // 量程为±2g时，灵敏度：16384 LSB/g  
  g_faccel_Z = accelZ/16384;             // 量程为±2g时，灵敏度：16384 LSB/g  
}
/*----------角速度数据-----------*/
void recordGyroRegisters() {
  Wire.beginTransmission(0x68);          //I2C 将数据写入的地址0x68
  Wire.write(0x43);                      //I2C向地址写入的数据0x43
  Wire.endTransmission();                //I2C结束写数据
  Wire.requestFrom(0x68,6);              //I2C从0x68地址读取6个数据
  while(Wire.available() < 6);           // 等待6个字节的数据全部存储好 （必须等待所有数据存储到缓冲区后才能读取） 
  gyroX = Wire.read()<<8|Wire.read();    //第一个字节和第二个字节是X轴的角速度数据
  gyroY = Wire.read()<<8|Wire.read();    //第一个字节和第二个字节是Y轴的角速度数据
  gyroZ = Wire.read()<<8|Wire.read();    //第一个字节和第二个字节是Z轴的角速度数据

  g_fGyro_X = gyroX/16.4;                //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
  g_fGyro_Y = gyroY/16.4;                //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
  g_fGyro_Z = gyroZ/16.4;                //范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
}

  
 
